#include "Agent.h"


Agent::Agent(ISceneManager* smgr, stringw meshPath, vector3df startPosition, vector3df orientation) : m_isIdle(true), m_moveRate(10){
	
	m_Node = smgr->addAnimatedMeshSceneNode(smgr->getMesh(meshPath));
	m_currentGoal = m_Node->getPosition();

	if(m_Node != 0)
	{
		m_Node->setPosition(startPosition);
		m_Node->setPosition(core::vector3df(2,0,2));
		m_Node->setScale(core::vector3df(1200,1200,1200));
		m_Node->setAnimationSpeed(0);
		m_Node->setMaterialFlag(video::EMF_NORMALIZE_NORMALS, true);
		m_Node->setMaterialFlag(video::EMF_LIGHTING, false);
		m_Node->setName("AGENT");
	}
}


Agent::~Agent(){};

void Agent::MoveTo(vector3df newGoal)
{
	printf("Agent has new move to goal of <%3.2f %3.2f %3.2f>\n", newGoal.X, newGoal.Y, newGoal.Z);
	m_currentGoal = newGoal;
}

bool Agent::IsIdle()const{
	return m_isIdle;
}

void Agent::SetPosition(vector3df newPosition)
{
	
	m_Node->setPosition(newPosition);
	m_isIdle=false;
}

vector3df Agent::GetPosition()const{
	return m_Node->getPosition();
}

vector3df Agent::GetRotation()const{
	return m_Node->getRotation();
}

vector3df Agent::GetCurrentGoal()const{
	return m_currentGoal;
}

void Agent::Update(u32 timeMs)
{
	if(m_Node->getPosition() == m_currentGoal){
		m_isIdle = true;
		m_Node->setAnimationSpeed(0);
		return;
	}
	
	m_isIdle = false;
	m_Node->setAnimationSpeed(10);
	vector3df newPosition;
	vector3df nodePosition = m_Node->getPosition();
	vector3df newRotation;
	vector3df currentRotation =  m_Node->getRotation();

	if(m_currentGoal.getDistanceFrom(nodePosition) < .5)
	{
		newPosition = m_currentGoal;
		m_isIdle = true;
		newRotation = (m_currentGoal - nodePosition).getHorizontalAngle();
		if(abs((newRotation.X - currentRotation.X)) > 180 || abs((newRotation.Y - currentRotation.Y)) > 180
			|| abs((newRotation.Z - currentRotation.Z)) > 180)
		{
			newRotation = currentRotation;
		}
	}
	else
	{
		vector3df target = m_currentGoal - nodePosition;
		newPosition = nodePosition + target.normalize();
		newRotation = target.getHorizontalAngle();
		if(abs((newRotation.X - currentRotation.X)) > 180 || abs((newRotation.Y - currentRotation.Y)) > 180
			|| abs((newRotation.Z - currentRotation.Z)) > 180)
		{
			newRotation = currentRotation;
		}
	}

	m_Node->setPosition(newPosition);
	m_Node->setRotation(newRotation);
}